#include "RadarSampler.h"
#include "OCesium/CesiumMath.h"
#include "OCesium/Cartesian3.h"
#include "OCMain/OCMath.h"

namespace OCPP
{
	namespace Volume
	{
		RadarSampler::RadarSampler()
		{

		}

		RadarSampler::~RadarSampler()
		{

		}

		void RadarSampler::setValue(int layer, int angle1, int dis, uint8 value)
		{
			auto& vecChar = _data._vecChar;
			auto dimXY = _imageWidth * _imageHeight;
			auto offset = layer * dimXY;
			auto size = offset + dimXY;
			if (vecChar.size() != size)
			{
				vecChar.resize(size);
			}

			auto setValue = [&vecChar,&offset, &value, this](int x, int y)
				{
					auto c = Cesium::CesiumMath::clamp(x, 0, _imageWidth);
					auto r = Cesium::CesiumMath::clamp(y, 0, _imageHeight);
					auto i = offset + (_imageHeight - 1 - r) * _imageWidth + c;
					if (vecChar[i] < value) vecChar[i] = value;
				};

			auto setInterpolation = [this,&setValue](int x,int y)
				{
					auto imageOffset = _radius;
					auto c1 = imageOffset + Math::Floor(x);
					auto r1 = imageOffset + Math::Floor(y);
					auto c2 = imageOffset + Math::Ceil(x);
					auto r2 = imageOffset + Math::Floor(y);
					auto c3 = imageOffset + Math::Floor(x);
					auto r3 = imageOffset + Math::Ceil(y);
					auto c4 = imageOffset + Math::Ceil(x);
					auto r4 = imageOffset + Math::Ceil(y);
					setValue(c1, r1);
					setValue(c2, r2);
					setValue(c3, r3);
					setValue(c4, r4);
				};

			auto angle = Cesium::CesiumMath::toRadians(angle1);
			auto dir = Cesium::Cartesian3(Math::Cos(angle), Math::Sin(angle));
			dir = Cesium::Cartesian3::normalize(dir);
			auto pos = Cesium::Cartesian3::multiplyByScalar(dir, dis);
			setInterpolation(pos.x, pos.y);
		}

		void RadarSampler::setValue(int layer, int angle,int dis, uint16 value)
		{
			uint8 clampValue = value;
			setValue(layer, angle, dis, clampValue);
		}
	}
}